// --------------------------------------------------------------------------------------------------------------------
// <copyright file="ManipulationManifest.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Test.Robotics.Manipulation
{
    using System;
    using System.Collections.Generic;
    using System.IO;
    using Microsoft.Robotics.Hardware.KukaLBR;
    using Microsoft.Robotics.Hardware.Mico;
    using Microsoft.Robotics.Hardware.Robotiq;
    using Microsoft.Robotics.Hardware.Runtime;
    using Microsoft.Robotics.Hardware.Runtime.Sensors;
    using Microsoft.Robotics.Hardware.Runtime.Robotiq;
    using Microsoft.Robotics.Hardware.Runtime.Robotis;
    using Microsoft.Robotics.Hardware.Runtime.UniversalRobots;
    using Microsoft.Robotics.Hardware.UniversalRobots;
    using Microsoft.Robotics.Hardware.Visualization.Robotiq;
    using Microsoft.Robotics.Interaction.Runtime.Input;
    using Microsoft.Robotics.Manipulation.Runtime;
    using Microsoft.Robotics.Manipulation.Runtime.Arm;
    using Microsoft.Robotics.Manipulation.Runtime.MotionControl;
    using Microsoft.Robotics.Manipulation.Visualization;
    using Microsoft.Robotics.Numerics;
    using Microsoft.Robotics.Runtime;
    using Microsoft.Robotics.Vision.Runtime;
    using Microsoft.Robotics.Visualization; 
    using Test.Robotics.Hardware.Robotiq;
    using Test.Robotics.Interaction;
    using Test.Robotics.Manipulation.Metrics;
    using Test.Robotics.Metrics;
    using Test.Robotics.RdsSim;
    
    /// <summary>
    /// Provides methods for creating manifests in a type-checked way.
    /// </summary>
    public static class ManipulationManifest
    {
        /// <summary>
        /// Create a complete manifest for arm and hand control using LBR and Robotiq
        /// </summary>
        /// <param name="useKrl"> Indicates whether or not to use KRL to generate motions. </param>
        /// <returns>The manifest</returns>
        public static Manifest CreateLBRRobotiqManifest(bool useKrl = false)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();
            Manifest manifest = ManipulationManifest.CreateStandardLbrManifest(views, useKrl);
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddCartesianMotionAgent(ref manifest, ArmModel.KUKA);
            AddRobotiqSModel(ref manifest, ref views, true);
            AddArmVisualizations(ref manifest, ref views);
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to test manipulation with default kuka hardware
        /// </summary>
        /// <param name="useKrl"> Indicates whether or not to use KRL to generate motion commands.</param>
        /// <returns>A manifest that enables manipulation testing on Kuka arm</returns>
        public static Manifest CreateKukaLbrManipulatorTestManifest(bool useKrl = false)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();
            Manifest manifest = ManipulationManifest.CreateBasicKukaLbrManifest(useKrl);
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddCartesianMotionAgent(ref manifest, ArmModel.KUKA);
            AddPicknPlaceControlProxies(manifest, "lbr-control");
            AddArmVisualizations(ref manifest, ref views);
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to test UR hardware
        /// </summary>
        /// <returns>A manifest that enables testing control of UR arm </returns>
        public static Manifest CreateURManipulatorTestManifest()
        {
            List<ViewDefinition> views = new List<ViewDefinition>();
            Manifest manifest = ManipulationManifest.CreateBasicURManifest();
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddArmVisualizations(ref manifest, ref views);
            AddJointControllerVisualization(ref manifest, ref views);
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create a manifest for the UR arm, gripper and proxies for pick and place
        /// </summary>
        /// <param name="visionAddress">Address of the vision machine</param>
        /// <param name="doAddProxy">Controls if proxies are set up</param>
        /// <returns>A manifest that enables testing control of UR arm </returns>
        public static Manifest CreateURManipulatorWithAX12TestManifest(string visionAddress, bool doAddProxy = true)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();
            string motionAgentName = "UR5MotionAgent";

            Manifest manifest = ManipulationManifest.CreateBasicURManifest();
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddCartesianMotionAgent(ref manifest, ArmModel.UR5, motionAgentName);
            AddAX12DualGripper(ref manifest, ref views);
            AddArmVisualizations(ref manifest, ref views, motionAgentName);
            if (true == doAddProxy)
            {
                AddPicknPlaceControlProxies(manifest, visionAddress);
            }
            AddUserInterface(ref manifest, views);

            return manifest;
        }


        /// <summary>
        /// Create a manifest to control the UR arm, no gripper and pick and place proxies
        /// </summary>
        /// <param name="visionAddress">Address of the vision machine</param>
        /// <returns>A manifest that enables testing UR arm </returns>
        public static Manifest CreateUR5SimpleCartesianMotionControlManifest(string visionAddress)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();

            string motionAgentName = "UR5MotionAgent";

            Manifest manifest = ManipulationManifest.CreateBasicURManifest();
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddCartesianMotionAgent(ref manifest, ArmModel.UR5, motionAgentName);
            AddArmVisualizations(ref manifest, ref views, motionAgentName);
            AddPicknPlaceControlProxies(manifest, visionAddress);
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to test manipulation KUKA LBR RDS sim.
        /// </summary>
        /// <returns>A manifest that enables manipulation testing on RDS sim of Kuka arm</returns>
        public static Manifest CreateKukaLbrSimManipulationTestManifest()
        {
            List<ViewDefinition> views = new List<ViewDefinition>();
            Manifest manifest = ManipulationManifest.CreateKukaLbrSimManipulationTestManifestBase();
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddArmVisualizations(ref manifest, ref views);
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create a manifest that creates agents and displays for Robotiq S Model and KUKA LBR
        /// and adds to the views
        /// </summary>
        /// <param name="views"> Views to add. </param>
        /// <param name="useKrl"> Indicates whether or not to use KRL to generate motion commands.</param>
        /// <returns> A manifest. </returns>
        public static Manifest CreateStandardLbrManifest(List<ViewDefinition> views, bool useKrl = false)
        {
            Manifest manifest = new Manifest();

            // Add KUKA LBR agents
            TimerAgent lbrTimer = new TimerAgent("ManipulationHeartbeatTimer", 15);
            manifest.Agents.Add(lbrTimer);

            // create a region accessibility checker
            RegionAccessibilityChecker rc = ManipulationManifest.CreateRegionAccessibilityChecker();

            KukaLBRControlAgent manipulationAgent = new KukaLBRControlAgent(
                        name: "ManipulationAgent",
                        producer: lbrTimer,
                        maxLinearVelocityInMetersPerSec: 0.1,
                        maxAngularVelocityInRadPerSec: 0.1,
                        maxJointVelocityRadPerSec: Math.PI / 4,
                        useKrlMotionCommands: useKrl,
                        jointMotionDurationInCycles: 11,
                        accessibilityChecker: rc);
            manifest.Agents.Add(manipulationAgent);
            manifest.ActiveAgents.Add(manipulationAgent.Name);

            return manifest;
        }

        /// <summary>
        /// Adds a cubic spline tracking agent to manifest
        /// </summary>
        /// <param name="manifest">Manifest to add agent to.</param>
        /// <param name="jointErrorLimit">Joint error limit for spline tracking</param>
        public static void AddCubicSplineTrackingAgent(ref Manifest manifest, double jointErrorLimit)
        {
            IAgent controlAgent = null;
            IProducer<ManipulatorStatusMessage> statusAgent = null;

            foreach (Agent a in manifest.Agents)
            {
                if (a is IControlReceiver<JointGoalPositionControlMessage>)
                {
                    controlAgent = a;
                }

                if (a is IProducer<ManipulatorStatusMessage>)
                {
                    statusAgent = a as IProducer<ManipulatorStatusMessage>;
                }
            }

            if (controlAgent == null)
            {
                throw new ApplicationException("Could not find IControlReceiver<ArmJointControlMessage> in manifest");
            }

            if (statusAgent == null)
            {
                throw new ApplicationException("Could not find IProducer<ManipulatorStatusMessage> in manifest");
            }

            ArmCubicSplineExecutionAgent cubicSplineAgent = new ArmCubicSplineExecutionAgent(
                name: "CubicSplineAgent",
                producer: statusAgent,
                armJointControlAgentName: controlAgent.Name,
                jointErrorLimit: jointErrorLimit);

            manifest.Agents.Add(cubicSplineAgent);
            manifest.ActiveAgents.Add(cubicSplineAgent.Name);
        }

        /// <summary>
        /// Add visualzations for viewing and controlling arm data.
        /// </summary>
        /// <param name="manifest">The manifest to work with</param>
        /// <param name="views">A list of views</param>
        /// <param name="armMotionAgentName">Name for arm motion agent</param>
        public static void AddArmVisualizations(ref Manifest manifest, ref List<ViewDefinition> views, string armMotionAgentName = null)
        {
            IProducer<ManipulatorStatusMessage> manipulationAgent = null;
            IAgent splineAgent = null;

            foreach (Agent a in manifest.Agents)
            {
                if (a is IProducer<ManipulatorStatusMessage>)
                {
                    manipulationAgent = a as IProducer<ManipulatorStatusMessage>;
                }

                if (a is ArmCubicSplineExecutionAgent)
                {
                    splineAgent = a;
                }
            }

            string splineAgentName = string.Empty;
            if (splineAgent != null)
            {
                splineAgentName = splineAgent.Name;
            }

            if (manipulationAgent == null)
            {
                throw new ApplicationException("Manifest does not contain a valid manipulation agent");
            }

            ArmPositionDisplayAgent displayModel = new ArmPositionDisplayAgent(
                name: "ArmDisplayModel",
                producer: manipulationAgent,
                maxUpdateIntervalMilliseconds: 400,
                manipulationAgentName: armMotionAgentName == null ? manipulationAgent.Name : armMotionAgentName,
                splineAgentName: splineAgentName,
                cartesianControlTolerance: 0.01,
                jointControlTolerance: 0.01);
            manifest.Agents.Add(displayModel);

            ViewDefinition armPositionDisplay = new ViewDefinition("ArmPositionDisplay", typeof(ArmPositionDisplayView), displayModel);
            ViewDefinition armControlViewDefinition = new ViewDefinition("ArmControlDisplay", typeof(ArmControlDisplayView), displayModel);
            views.Add(armPositionDisplay);
            views.Add(armControlViewDefinition);
        }

        /// <summary>
        /// Add visualzations for testing and tuning joint controllers
        /// </summary>
        /// <param name="manifest">The manifest to work with</param>
        /// <param name="manipulationAgent">The maniuplation agent</param>
        /// <param name="splineAgent">An agent for a spline tracker</param>
        public static void AddJointControllerVisualization(ref Manifest manifest, ref List<ViewDefinition> views)
        {
            IProducer<ManipulatorStatusMessage> manipulatorAgent = null;

            foreach (Agent a in manifest.Agents)
            {
                if (a is IProducer<ManipulatorStatusMessage>)
                {
                    manipulatorAgent = a as IProducer<ManipulatorStatusMessage>;
                }
            }

            if (manipulatorAgent == null)
            {
                throw new ApplicationException("Manifest does not contain a valid manipulation agent");
            }

            JointControllerDisplayAgent displayModel = new JointControllerDisplayAgent(
                name: "JointControllerDisplayModel",
                producer: manipulatorAgent,
                maxUpdateIntervalMilliseconds: 400,
                armControlAgentName: manipulatorAgent.Name,
                numJoints: 6);

            manifest.Agents.Add(displayModel);

            ViewDefinition jointControllerDisplay = new ViewDefinition("JointControllerDisplay", typeof(JointControllerDisplayView), displayModel);

            views.Add(jointControllerDisplay);
        }

        /// <summary>
        /// Add a user interface to the manifest
        /// </summary>
        /// <param name="manifest"></param>
        /// <param name="views"></param>
        public static void AddUserInterface(ref Manifest manifest, List<ViewDefinition> views)
        {
            // add the UI
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);
            manifest.ActiveAgents.Add(userInterface.Name);
        }

        /// <summary>
        /// Adds the RobotiqSModel gripper and optionally visualization.  Does not add user interface agent
        /// </summary>
        /// <param name="manifest">Manifest to add gripper and (optionally) visualization to.</param>
        /// <param name="addVisualization">Indicates whether or not to add visualzation.</param>
        public static void AddRobotiqSModel(ref Manifest manifest, ref List<ViewDefinition> views, bool addVisualization = true)
        {
            // Add Robotiq agents
            TimerAgent robotiqTimer = new TimerAgent(
               name: "RobotiqHeartbeatTimer",
               timerInterval: 20);
            manifest.Agents.Add(robotiqTimer);

            RobotiqSModelAgent robotiqAgent = new RobotiqSModelAgent(
                name: "GripperAgent",
                producer: robotiqTimer);
            manifest.Agents.Add(robotiqAgent);
            manifest.ActiveAgents.Add(robotiqAgent.Name);

            RobotiqSModelVisualizationAgent robotiqModelAgent = new RobotiqSModelVisualizationAgent(
                name: "RobotiqSModelVisualizationAgent",
                producer: robotiqAgent,
                maxUpdateIntervalMilliseconds: 20,
                robotiqAgentName: robotiqAgent.Name);
            manifest.Agents.Add(robotiqModelAgent);

            // Add displays
            ViewDefinition def = new ViewDefinition("RobotiqSModelDisplay", typeof(RobotiqSModelDisplayView), robotiqModelAgent);
            views.Add(def);
        }

        /// <summary>
        /// Adds the AX12 dual gripper and optionally visualization.  Does not add user interface agent
        /// </summary>
        /// <param name="manifest">Manifest to add gripper and (optionally) visualization to.</param>
        /// <param name="addVisualization">Indicates whether or not to add visualzation.</param>
        public static void AddAX12DualGripper(ref Manifest manifest, ref List<ViewDefinition> views, bool addVisualization = true)
        {
            // Add Robotiq agents
            TimerAgent ax12Timer = new TimerAgent(
               name: "AX12GripperHeartbeatTimer",
               timerInterval: 20);
            manifest.Agents.Add(ax12Timer);

            AX12DualGripperAgent ax12Agent = new AX12DualGripperAgent(
                name: "GripperAgent",
                producer: ax12Timer);
            manifest.Agents.Add(ax12Agent);
            manifest.ActiveAgents.Add(ax12Agent.Name);

            RobotiqSModelVisualizationAgent robotiqModelAgent = new RobotiqSModelVisualizationAgent(
                name: "RobotiqSModelVisualizationAgent",
                producer: ax12Agent,
                maxUpdateIntervalMilliseconds: 20,
                robotiqAgentName: ax12Agent.Name);
            manifest.Agents.Add(robotiqModelAgent);

            // Add displays
            ViewDefinition def = new ViewDefinition("AX12DualGripperDisplay", typeof(RobotiqSModelDisplayView), robotiqModelAgent);
            views.Add(def);
        }

        /// <summary>
        /// Add a Cartesian motion agent to manifest.
        /// </summary>
        /// <param name="manifest">Manifest to add agent to.</param>
        /// <param name="armModel">The model of the arm.</param>
        /// <param name="motionAgentName">Name of the motion agent</param>
        /// <param name="visualizerName">Name fo the visualizer</param>
        public static void AddCartesianMotionAgent(ref Manifest manifest, ArmModel armModel, string motionAgentName = null, string visualizerName = null)
        {
            // Create a proxy to connect ot the kinect
            //// string kinectV2Address = "mrevow7";
            List<ViewDefinition> views = new List<ViewDefinition>();

            ////ProxyDefinition kinectProcessorV2 = ProxyDefinition.CreateRemoteProxy(
            ////                name: "kinectV2Processor",
            ////                address: kinectV2Address,
            ////                publishContracts: new[] { typeof(KinectAgentMessage) },
            ////                publishNetworkPortNumber: (int)RemotingPorts.KinectRawPublish,
            ////                controlContracts: null,
            ////                controlNetworkPortNumber: 0);

            ////manifest.RemoteAgentProxies.Add(kinectProcessorV2);

            IProducer<ManipulatorStatusMessage> manipulationAgent = null;
            IControllable<StartTrajectoryMessage> cubicSplineAgent = null;

            foreach (Agent a in manifest.Agents)
            {
                if (a is IControllable<StartTrajectoryMessage>)
                {
                    cubicSplineAgent = a as IControllable<StartTrajectoryMessage>;
                }

                if (a is IProducer<ManipulatorStatusMessage>)
                {
                    manipulationAgent = a as IProducer<ManipulatorStatusMessage>;
                }
            }

            if (manipulationAgent == null)
            {
                throw new ApplicationException("No manipulation agent found");
            }

            if (cubicSplineAgent == null)
            {
                throw new ApplicationException("No cubic spline execution agent found");
            }

            // Create a simple cartesian motion agent
            SimpleCartesianMoveAgent motionAgent = null;

            if (ArmModel.KUKA == armModel)
            {
                motionAgent = new KukaLBRSimpleCartesianMoveAgent(
                    name: motionAgentName == null ? "KUKASimpleCartesianMoveAgent" : motionAgentName,
                    producer: manipulationAgent,
                    trajectoryControlAgent: cubicSplineAgent,
                    jointTolerance: 0.01,
                    minimumJointHeight: 0.08,
                    maximumJointVel: Math.PI / 4,
                    cutOffPercentageForIKFiltering: 0.8,
                    minimumTrajectoryTime: 2.0,
                    guaranteedClearance: 0.01,
                    minimumElbowHeight: 0.2,
                    trajectoryQualityThreshold: 10.0,
                    jointWeightFactors: new double[] { 1, 1.5, 1, 1, 1.5, 1, 0.5 },
                    elbowClearanceCheckList: new bool[] { false, false, true, false, false, false, false },
                    visualizerAgentName: visualizerName);
            }
            else if (ArmModel.UR5 == armModel)
            {
                motionAgent = new URSimpleCartesianMoveAgent(
                    name: motionAgentName == null ? "URSimpleCartesianMoveAgent" : motionAgentName,
                    producer: manipulationAgent,
                    trajectoryControlAgent: cubicSplineAgent,
                    jointTolerance: 0.01,
                    minimumJointHeight: 0.08,
                    maximumJointVel: Math.PI / 4,
                    cutOffPercentageForIKFiltering: 0.8,
                    minimumTrajectoryTime: 2.0,
                    guaranteedClearance: 0.01,
                    minimumElbowHeight: 0.2,
                    trajectoryQualityThreshold: 10.0,
                    jointWeightFactors: new double[] { 1, 1, 1, 1, 1, 1 },
                    elbowClearanceCheckList: new bool[] { false, false, true, false, false, false },
                    visualizerAgentName: visualizerName);
            }

            manifest.Agents.Add(motionAgent);
            manifest.ActiveAgents.Add(motionAgent.Name);
        }

        /// <summary>
        /// Create a manifest that creates agents and displays for Robotiq S Model and KUKA LBR
        /// </summary>
        /// <param name="startPosition">Start pose for motion.</param>
        /// <param name="endPosition">End pose of motion (assuming no contact)</param>
        /// <param name="iterationCount">Number of iterations to run</param>
        /// <param name="expectedTarget">Expected pose of contact.</param>
        /// <param name="linearSpeed">Linear speed to move setpoint at.</param>
        /// <param name="rotationalSpeed">Rotational speed to move setpoint at.</param>
        /// <param name="forceThreshold">Force threshold for stopping on contact</param>
        /// <param name="forceAxesMask">Mask for which axes to report residual forces for</param>
        /// <param name="positionAxesMask">Mask for which axes to report postion deviations for</param>
        /// <param name="dataCollectionDelayInMs">Pause between iterations for data collection</param>
        /// <returns></returns>
        public static Manifest CreateLinearTouchMetricManifest(
            Pose startPosition,
            Pose endPosition,
            int iterationCount,
            Pose expectedTarget,
            double linearSpeed,
            double rotationalSpeed,
            double forceThreshold,
            Vector3 forceAxesMask,
            Vector3 positionAxesMask,
            long dataCollectionDelayInMs)
        {
            Manifest manifest = new Manifest();

            TimerAgent timer = new TimerAgent(
               name: "ManipulationTimer",
               timerInterval: 15);
            manifest.Agents.Add(timer);

            KukaLBRControlAgent manipulationAgent = new KukaLBRControlAgent(
                        name: "ManipulationAgent",
                        producer: timer,
                        maxLinearVelocityInMetersPerSec: linearSpeed,
                        maxAngularVelocityInRadPerSec: rotationalSpeed,
                        maxJointVelocityRadPerSec: 0.1,
                        jointMotionDurationInCycles: 11,
                        useKrlMotionCommands: false);
            manifest.Agents.Add(manipulationAgent);

            timer = new TimerAgent(
               name: "ExecuteSingleLinearTouchAgentTimer",
               timerInterval: 50);
            manifest.Agents.Add(timer);

            ExecuteSingleLinearTouchAgent touchAgent = new ExecuteSingleLinearTouchAgent(
                name: "TouchAgent",
                producer: timer,
                manipulationAgentName: "ManipulationAgent",
                startPose: startPosition,
                targetPose: endPosition,
                forceThresholdInNewtons: forceThreshold,
                linearSpeedInMetersPerSecond: linearSpeed,
                rotationalSpeedInRadiansPerSecond: rotationalSpeed);
            manifest.Agents.Add(touchAgent);

            StopOnContactMetricsCalculator calcAgent = new StopOnContactMetricsCalculator(
                name: "StopOnConctactCalculator",
                producer: manipulationAgent,
                taskControllerName: "TouchAgent",
                positionAxesMask: positionAxesMask,
                forceAxesMask: forceAxesMask,
                idealPosition: expectedTarget.Position,
                robotIDString: "LBR224",
                dataCollectionDelayInMs: dataCollectionDelayInMs);
            manifest.Agents.Add(calcAgent);

            MetricsController controller = new MetricsController(
                name: "controller",
                calculatorAgent: calcAgent,
                completionAgents: new[] { calcAgent },
                iterations: iterationCount);
            manifest.Agents.Add(controller);

            ManipulatorStatusMessageConsumer manipulationAgentKeepAlive = new ManipulatorStatusMessageConsumer(
                name: "ManipulatorStatusMessageConsumer",
                producer: manipulationAgent);
            manifest.Agents.Add(manipulationAgentKeepAlive);

            // Activate agents.
            manifest.ActiveAgents.Add(manipulationAgentKeepAlive.Name);
            manifest.ActiveAgents.Add(controller.Name);
            return manifest;
        }

        /// <summary>
        /// Create a pick and place using the CNC proxy
        /// </summary>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <returns>The manifest</returns>
        public static Manifest CreateCNCPickAndPlaceManifest(string robotConfigFolder)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();

            Manifest manifest = ManipulationManifest.CreateStandardLbrManifest(views, true);
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddCartesianMotionAgent(ref manifest, ArmModel.KUKA);
            AddRobotiqSModel(ref manifest, ref views, true);
            AddArmVisualizations(ref manifest, ref views); 

            string armAgentName = null;
            string handAgentName = null;
            string motionAgentName = null;
            string picknPlaceAgentName = "PicknPlaceAgent";
            string picknPlaceProxyName = "picknPlaceProxy";
            string pickPlaceObjectDetectorIntegratorName = "picknPlaceObjectDetectorIntegrator";
            string kinectV2Address = "mrevow4";
            string [] detectorConfigFiles = new string[] { @"Data\ObjectDetection\Configurations\tomatoCanDetector_v3.xml", @"Data\ObjectDetection\Configurations\soapBoxDetector_v3.xml", @"Data\ObjectDetection\Configurations\potatoCanDetector_v3.xml" };

            IProducer<ManipulatorStatusMessage> armAgent = null;

            foreach (Agent agent in manifest.Agents)
            {
                if (agent is KukaLBRControlAgent)
                {
                    armAgentName = agent.Name;
                    armAgent = agent as IProducer<ManipulatorStatusMessage>;
                }
                else if (agent.GetType() == typeof(RobotiqSModelAgent))
                {
                    handAgentName = agent.Name;
                }
                else if (agent is KukaLBRSimpleCartesianMoveAgent)
                {
                    motionAgentName = agent.Name;
                }
            }

            if (true == string.IsNullOrEmpty(armAgentName))
            {
                throw new Exception("Did not find the arm control agent");
            }

            if (true == string.IsNullOrEmpty(handAgentName))
            {
                throw new Exception("Did not find the hand control agent");
            }

            PicknPlaceAgentKukaRobotiq pickPlaceAgent = new PicknPlaceAgentKukaRobotiq(
                                                        name: picknPlaceAgentName,
                                                        producer: armAgent,
                                                        armControlAgentName: armAgentName,
                                                        handControlAgentName: handAgentName,
                                                        handStatusProvider: handAgentName,
                                                        armMoveRotationalTolerance: 0.025 * 5.0,
                                                        armMoveTranslationalTolerance: 0.025,
                                                        liftHeight: 0.170,
                                                        defaultSubTaskStepTimeOutmSec: 10,
                                                        gripExchangePose: new Pose(new Vector3(-0.45, 0.57, 0.005), new Quaternion(0.0, 0.0, 0.7285, -0.6851)),
                                                        defaultNeutralPose: new Pose(new Vector3(-0.68, 0.0, 0.15)));

            manifest.Agents.Add(pickPlaceAgent);

            KinectObjectDetectorIRAgent objectDetector = Test.Robotics.Vision.VisionManifest.CreateObjectDetectionManifest(robotConfigFolder, manifest, views, kinectV2Address, detectorConfigFiles);

            ObjectDetectorPicknPlaceIntegratorAgent objectDetectorIntegrator = new ObjectDetectorPicknPlaceIntegratorAgent(
                                                name: pickPlaceObjectDetectorIntegratorName,
                                                producer: pickPlaceAgent,
                                                objectDetectorAgentName: objectDetector.Name,
                                                objectDetectorDataCollectionAgentName: string.Empty,
                                                minConfidence: 0.9,
                                                maxRetry: 4,
                                                minDetectionCount: 1);
            manifest.Agents.Add(objectDetectorIntegrator);

            ProxyDefinition armProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                        name: picknPlaceProxyName,
                        producer: objectDetectorIntegrator.Name,
                        address: "10.137.176.98",           // Bug Requires an IPV4 address Bug 1649
                        publishContracts: new[] { typeof(PicknPlaceStatusMessage) },
                        publishNetworkPortNumber: (int)RemotingPorts.PicknPlacePublish,
                        controlContracts: new[] { typeof(PickAndMoveMessage), typeof(PlaceAndMoveMessage) },
                        controlNetworkPortNumber: (int)RemotingPorts.PicknPlaceControl);

            manifest.NetworkEnabledAgentProxies.Add(armProxy);
            manifest.ActiveAgents.Add(pickPlaceAgent.Name);
            manifest.ActiveAgents.Add(armProxy.Name);
            manifest.ActiveAgents.Add(objectDetectorIntegrator.Name);

            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create the vision part of a pick and place using the CNC proxy
        /// </summary>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="useKrl">When true use KRL control</param>
        /// <returns>The manifest</returns>
        public static Manifest CreateCNCPickAndPlaceVisionManifest(string robotConfigFolder, ArmModel armModel = ArmModel.KUKA, bool useKrl = true)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();
            Manifest manifest = new Manifest();
            ManipulationManifest.AddPickAndPlaceBasic(robotConfigFolder, manifest, views, "lbr-ctrl-win7", armModel, useKrl);
            IProducer<PicknPlaceStatusMessage> multiPickPlace = ManipulationManifest.AddMultiPicknPlaceToManifest(manifest, robotConfigFolder, views);
            IProducer<PicknPlaceStatusMessage> objectDetectorIntegrator = ManipulationManifest.AddObjectDetectionToManifest(manifest, multiPickPlace, robotConfigFolder, views);
            ManipulationManifest.AddPicknPlaceCNCProxy(manifest, objectDetectorIntegrator, "cnc-testbed2");
            ManipulationManifest.AddVisuals(manifest, views);

            return manifest;
        }

        /// <summary>
        /// Add basic pick and place functionality to a manifest
        /// </summary>
        /// <param name="robotConfigFolder">Calibration Data Folder</param>
        /// <param name="manifest">Manifest to add to</param>
        /// <param name="views">Views for basic visualization</param>
        /// <param name="armControlIpAddress">IP address for arm controller machine</param>
        /// <param name="useKrl">When true use KRL control. Only relevant for the LBR arm, ignored for other arms.</param>
        /// <returns>Pick and place agent</returns>
        public static IProducer<PicknPlaceStatusMessage> AddPickAndPlaceBasic(string robotConfigFolder, Manifest manifest, List<ViewDefinition> views, string armControlIpAddress, ArmModel armModel, bool useKrl)
        {
            string armControlProxyName = "armControlProxy";
            string handControlProxyName = "handControlProxy";
            string motionPlanProxyName = "motionPlanProxy";

            ProxyDefinition handControlProxy = ProxyDefinition.CreateRemoteProxy(
                name: handControlProxyName,
                address: armControlIpAddress,
                publishContracts: new[] { typeof(RobotiqSModelStateMessage) },
                publishNetworkPortNumber: (int)RemotingPorts.HandPublish,
                controlContracts: new[] { typeof(RobotiqSModelControlMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.HandControl);

            ProxyDefinition armControlProxy = ProxyDefinition.CreateRemoteProxy(
                name: armControlProxyName,
                address: armControlIpAddress,
                publishContracts: new[] { typeof(ManipulatorStatusMessage) },
                publishNetworkPortNumber: (int)RemotingPorts.ArmPublish,
                controlContracts: new[] { typeof(CartesianGoalPositionControlMessage), typeof(LinearCartesianMotionControlMessage), typeof(JointGoalPositionControlMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.ArmControl);

            ProxyDefinition motionPlanProxy = null;
            if (false == useKrl)
            {
                motionPlanProxy = ProxyDefinition.CreateRemoteProxy(
                name: motionPlanProxyName,
                address: armControlIpAddress,
                publishContracts: null,
                publishNetworkPortNumber: (int)RemotingPorts.MotionPlanPublish,
                controlContracts: new[] { typeof(CartesianGoalPositionControlMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.MotionPlanControl);
            }

            RobotiqSModelVisualizationAgent robotiqModelAgent = new RobotiqSModelVisualizationAgent(
                    name: "RobotiqSModelVisualizationAgent",
                    producer: handControlProxy.AsProducer<RobotiqSModelStateMessage>(),
                    maxUpdateIntervalMilliseconds: 20,
                    robotiqAgentName: handControlProxy.Name);

            manifest.Agents.Add(robotiqModelAgent);

            ArmPositionDisplayAgent displayModel = new ArmPositionDisplayAgent(
                   name: "ArmDisplayModel",
                   producer: armControlProxy.AsProducer<ManipulatorStatusMessage>(),
                   maxUpdateIntervalMilliseconds: 400,
                   manipulationAgentName: useKrl == true ? armControlProxy.Name : motionPlanProxy.Name,
                   splineAgentName: string.Empty,
                   cartesianControlTolerance: 0.01,
                   jointControlTolerance: 0.01);

            manifest.Agents.Add(displayModel);

            ViewDefinition def = new ViewDefinition("RobotiqSModelDisplay", typeof(RobotiqSModelDisplayView), robotiqModelAgent);
            views.Add(def);

            def = new ViewDefinition("LBRDisplay", typeof(ArmPositionDisplayView), displayModel);
            views.Add(def);

            manifest.ActiveAgents.Add(displayModel.Name);
            manifest.RemoteAgentProxies.Add(armControlProxy);
            manifest.RemoteAgentProxies.Add(handControlProxy);
            manifest.ActiveAgents.Add(armControlProxy.Name);
            manifest.ActiveAgents.Add(handControlProxy.Name);

            if (false == useKrl)
            {
                manifest.RemoteAgentProxies.Add(motionPlanProxy);
                manifest.ActiveAgents.Add(motionPlanProxy.Name);
            }

            string picknPlaceAgentName = "PicknPlaceAgent";
            PicknPlaceAgentKukaRobotiq pickPlaceAgent = null;
            if (ArmModel.KUKA == armModel)
            {
                pickPlaceAgent = new PicknPlaceAgentKukaRobotiq(
                                                        name: picknPlaceAgentName,
                                                        producer: armControlProxy.AsProducer<ManipulatorStatusMessage>(),
                                                        armControlAgentName: useKrl == true ? armControlProxy.Name : motionPlanProxy.Name,
                                                        handControlAgentName: handControlProxyName,
                                                        handStatusProvider: handControlProxyName,
                                                        armMoveRotationalTolerance: 0.025 * 5.0,
                                                        armMoveTranslationalTolerance: 0.025,
                                                        liftHeight: 0.190,
                                                        defaultSubTaskStepTimeOutmSec: 10,
                                                        gripExchangePose: new Pose(new Vector3(-0.45, 0.57, 0.0), new Quaternion(0.0, 0.0, 0.7285, -0.6851)),
                                                        defaultNeutralPose: new Pose(new Vector3(-0.68, 0.0, 0.15)));
            }
            else if (ArmModel.UR5 == armModel)
            {
                pickPlaceAgent = new PicknPlaceAgentUr5Ax12(
                                        name: picknPlaceAgentName,
                                        producer: armControlProxy.AsProducer<ManipulatorStatusMessage>(),
                                        armControlAgentName: useKrl == true ? armControlProxy.Name : motionPlanProxy.Name,
                                        handControlAgentName: handControlProxyName,
                                        handStatusProvider: handControlProxyName,
                                        armMoveRotationalTolerance: 0.025 * 5.0,
                                        armMoveTranslationalTolerance: 0.005,
                                        liftHeight: 0.190,
                                        defaultSubTaskStepTimeOutmSec: 10,
                                        gripExchangePose: new Pose(new Vector3(-0.3, 0.45, 0.0), new Quaternion(1, 0.0, 0.0, 0.0)),
                                        defaultNeutralPose: new Pose(new Vector3(-0.3, 0.45, 0.365), new Quaternion(0, 0, 0, 1)));
            }
            else
            {
                throw new ArgumentException("Invalid arm model specified for AddPickAndPlaceBasic()");
            }

            manifest.Agents.Add(pickPlaceAgent);
            manifest.ActiveAgents.Add(pickPlaceAgent.Name);

            return pickPlaceAgent;
        }

        /// <summary>
        /// Add multi pick and place to a manifest
        /// </summary>
        /// <param name="manifest">Manifest to add to</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="views">Views for basic visualization</param>
        /// <returns>Multi pick and place agent</returns>
        public static IProducer<PicknPlaceStatusMessage> AddMultiPicknPlaceToManifest(Manifest manifest, string robotConfigFolder, List<ViewDefinition> views)
        {
            IProducer<PicknPlaceStatusMessage> pickPlaceAgent = null;
            foreach (Agent agent in manifest.Agents)
            {
                if (agent is IProducer<PicknPlaceStatusMessage>)
                {
                    pickPlaceAgent = agent as IProducer<PicknPlaceStatusMessage>;
                    break;
                }
            }

            if (null == pickPlaceAgent)
            {
                throw new ArgumentException("No producer of PicknPlaceStatusMessage in manifest");
            }

            MultiPicknPlaceAgent multiAgent = new MultiPicknPlaceAgent(
                        name: "MultiPicknPlaceAgent",
                        producer: pickPlaceAgent,
                        startPoseOnTable: new Pose(new Vector3(0, 0.57, 0.0), new Quaternion(0.0, 0.0, 0.7285, -0.6851)),
                        armReferencePosition: new Vector3(-0.0, 0.0, 0),
                        cncLengths: new Vector3(0.24, 0.37, 0));

            manifest.Agents.Add(multiAgent);

            return multiAgent;
        }

        /// <summary>
        /// Add object detection to a pick and place manifest
        /// </summary>
        /// <param name="manifest">Manifest to add to</param>
        /// <param name="robotConfigFolder">Robot config folder</param>
        /// <param name="views">Views for basic visualization</param>
        /// <returns>Added object integrator agent</returns>
        public static ObjectDetectorPicknPlaceIntegratorAgent AddObjectDetectionToManifest(Manifest manifest, IProducer<PicknPlaceStatusMessage> pickPlaceAgent, string robotConfigFolder, List<ViewDefinition> views)
        {
            string pickPlaceObjectDetectorIntegratorName = "picknPlaceObjectDetectorIntegrator";
            string kinectV2Address = "mrevow4";
            string[] detectorConfigFiles = new string[] { @"Data\ObjectDetection\Configurations\tomatoCanDetector_v3.xml", @"Data\ObjectDetection\Configurations\soapBoxDetector_v3.xml", @"Data\ObjectDetection\Configurations\potatoCanDetector_v3.xml" };

            KinectObjectDetectorIRAgent objectDetector = Test.Robotics.Vision.VisionManifest.CreateObjectDetectionManifest(robotConfigFolder, manifest, views, kinectV2Address, detectorConfigFiles);

            ObjectDetectorPicknPlaceIntegratorAgent objectDetectorIntegrator = new ObjectDetectorPicknPlaceIntegratorAgent(
                                                name: pickPlaceObjectDetectorIntegratorName,
                                                producer: pickPlaceAgent,
                                                objectDetectorAgentName: objectDetector.Name,
                                                objectDetectorDataCollectionAgentName: string.Empty,
                                                minConfidence: 0.8,
                                                maxRetry: 4,
                                                minDetectionCount: 1,
                                                masterLogPathName: null,
                                                kinectFilesPath: null,
                                                expectedObjects: new List<string>() {"TomatoCan", "SoapBox", "PotatoCan"});

            manifest.Agents.Add(objectDetectorIntegrator);
            manifest.ActiveAgents.Add(objectDetectorIntegrator.Name);

            return objectDetectorIntegrator;
        }

        /// <summary>
        /// Add proxy between the pick and place machine and the CNC test machine
        /// </summary>
        /// <param name="manifest">Manifest to add to</param>
        /// <param name="pickPlaceAgent">The pick and place agent</param>
        /// <param name="cncIpAddress">IP address of the CNC machine</param>
        public static void AddPicknPlaceCNCProxy(Manifest manifest, IProducer<PicknPlaceStatusMessage> pickPlaceAgent, string cncIpAddress)
        {
            string picknPlaceProxyName = "picknPlaceProxy";

            ProxyDefinition armProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                        name: picknPlaceProxyName,
            producer: pickPlaceAgent.Name,
                        address: cncIpAddress,           // Bug Requires an IPV4 address Bug 1649
                        publishContracts: new[] { typeof(PicknPlaceStatusMessage), typeof(KinectObjectDetectorResultIRMessage) },
                        publishNetworkPortNumber: (int)RemotingPorts.PicknPlacePublish,
                        controlContracts: new[] { typeof(PickAndMoveMessage), typeof(PlaceAndMoveMessage) },
                        controlNetworkPortNumber: (int)RemotingPorts.PicknPlaceControl);

            manifest.NetworkEnabledAgentProxies.Add(armProxy);
            manifest.ActiveAgents.Add(armProxy.Name);
        }


        public static void AddVisuals(Manifest manifest, List<ViewDefinition> views)
        {
            UserInterfaceAgent userInterface = new UserInterfaceAgent("UserInterface", views.ToArray());
            manifest.Agents.Add(userInterface);
            manifest.ActiveAgents.Add(userInterface.Name);
        }

        /// <summary>
        /// Create the control part of a pick and place using the CNC proxy
        /// </summary>
        /// <param name="visualizerIPAddress">IP address for visualizer</param>
        /// <returns>The manifest</returns>
        public static Manifest CreateCNCPickAndPlaceControlManifest(string visualizerIPAddress)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();

            Manifest manifest = ManipulationManifest.CreateStandardLbrManifest(views, true);
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddCartesianMotionAgent(ref manifest, ArmModel.KUKA);
            AddRobotiqSModel(ref manifest, ref views, true);
            AddArmVisualizations(ref manifest, ref views);

            AddProxyForRemoteVisualizer(manifest, visualizerIPAddress);

            AddPicknPlaceControlProxies(manifest, "10.137.176.219");
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create the arm control components for UR
        /// </summary>
        /// <returns>The manifest</returns>
        public static Manifest CreateURControlAndVisualizationManifest(string visualizerIPAddress)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();

            Manifest manifest = ManipulationManifest.CreateBasicURManifest();
            AddProxyForRemoteVisualizer(manifest, visualizerIPAddress);
            AddCubicSplineTrackingAgent(ref manifest, 0);
            AddCartesianMotionAgent(ref manifest, ArmModel.UR5, null, null);
            AddArmVisualizations(ref manifest, ref views);
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Setup local proxy for publishing messages to remote visualizer
        /// Setup the proxy to send control messages to remote visualizer
        /// </summary>
        /// <param name="manifest">The manifest</param>
        /// <param name="armVisualizerServerIP">The IP of remote visualizer</param>
        public static void AddProxyForRemoteVisualizer(Manifest manifest, string armVisualizerServerIP)
        {
            IProducer<ManipulatorStatusMessage> manipulatorStatusAgent = null;

            foreach (Agent a in manifest.Agents)
            {
                if (a is IProducer<ManipulatorStatusMessage>)
                {
                    manipulatorStatusAgent = a as IProducer<ManipulatorStatusMessage>;
                }
            }

            if (null == manipulatorStatusAgent)
            {
                throw new InvalidOperationException("No manipulator status agent found");
            }

            // for publishing subscription messages
            string publishToArmVisualizerProxyName = "publishToArmVisualizerProxy";
            ProxyDefinition publishToArmVisualizerProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: publishToArmVisualizerProxyName,
                producer: manipulatorStatusAgent.Name,
                address: armVisualizerServerIP,
                publishContracts: new[] { typeof(ManipulatorStatusMessage) },
                publishNetworkPortNumber: (int)RemotingPorts.Arm2ArmVisualizerPublish,
                controlContracts: null, // new[] { typeof(CartesianGoalPositionControlMessage), typeof(LinearCartesianMotionControlMessage), typeof(JointGoalPositionControlMessage), typeof(LinearCartesianTouchControlMessage), typeof(LinearCartesianPushControlMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.Arm2ArmVisualizerControl);
            manifest.NetworkEnabledAgentProxies.Add(publishToArmVisualizerProxy);
            manifest.ActiveAgents.Add(publishToArmVisualizerProxy.Name);
        }

        /// <summary>
        /// a visualizer that sits on the control desktop
        /// 1. display the arm according to the arm manipulation status message
        /// 2. accepts control messages to show individual simulation
        /// </summary>
        /// <param name="model">Arm model</param>
        /// <param name="armControlIpAddress">Ip address for arm control</param>
        /// <returns>A manifest</returns>
        public static Manifest CreateStandAloneVisualizerServerLocalManifest(ArmModel model, string armControlIpAddress)
        {
            string armControlProxyName = "armControlProxy";

            List<ViewDefinition> views = new List<ViewDefinition>();
            Manifest manifest = new Manifest();

            TimerAgent timer = new TimerAgent("HeartBeatTimer", 20);
            manifest.Agents.Add(timer);

            VisualizerAgent envDisplayAgent = new VisualizerAgent("EnvironmentDisplay", timer, 20);
            manifest.Agents.Add(envDisplayAgent);

            // incoming arm manipulator status
            ProxyDefinition armControlProxy = ProxyDefinition.CreateRemoteProxy(
                name: armControlProxyName,
                address: armControlIpAddress,
                publishContracts: new[] { typeof(ManipulatorStatusMessage) },
                publishNetworkPortNumber: (int)RemotingPorts.Arm2ArmVisualizerPublish,
                controlContracts: new[] { typeof(CartesianGoalPositionControlMessage), typeof(LinearCartesianMotionControlMessage), typeof(JointGoalPositionControlMessage), typeof(LinearCartesianTouchControlMessage), typeof(LinearCartesianPushControlMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.Arm2ArmVisualizerControl);
            manifest.RemoteAgentProxies.Add(armControlProxy);

            ArmVisualizationAgent armVisualizationUpdateAgent = null;
            if (ArmModel.KUKA == model)
            {
                armVisualizationUpdateAgent = new ArmVisualizationAgent("ArmVisualizer", armControlProxy.AsProducer<ManipulatorStatusMessage>(), envDisplayAgent, KukaLBR4KinematicConfigurations.CreateKuka7DOFKinematicChain());
            }
            else if (ArmModel.UR5 == model)
            {
                armVisualizationUpdateAgent = new ArmVisualizationAgent("ArmVisualizer", armControlProxy.AsProducer<ManipulatorStatusMessage>(), envDisplayAgent, URKinematicConfigurations.CreateUR5KinematicChain());
            }
            else if (ArmModel.MICO == model)
            {
                armVisualizationUpdateAgent = new ArmVisualizationAgent("ArmVisualizer", armControlProxy.AsProducer<ManipulatorStatusMessage>(), envDisplayAgent, MicoKinematicConfigurations.CreateMicoKinematicChain());
            }
            else
            {
                throw new Exception("Arm model not implemented for visualization");
            }

            manifest.Agents.Add(armVisualizationUpdateAgent);

            EnvironmentVisualizationAgent envVisualizationAgent = new EnvironmentVisualizationAgent("EnvironmentVisualizationAgent", envDisplayAgent.Name);
            manifest.Agents.Add(envVisualizationAgent);

            ViewDefinition environmentViewDefinition = new ViewDefinition("EnvironmentDisplay", typeof(EnvironmentDisplayView), envDisplayAgent);
            views.Add(environmentViewDefinition);
            AddUserInterface(ref manifest, views);

            manifest.ActiveAgents.Add(timer.Name);
            manifest.ActiveAgents.Add(envDisplayAgent.Name);
            manifest.ActiveAgents.Add(envVisualizationAgent.Name);
            manifest.ActiveAgents.Add(armControlProxy.Name);
            manifest.ActiveAgents.Add(armVisualizationUpdateAgent.Name);

            return manifest;
        }

        /// <summary>
        /// Adds the network proxies for pick and place control
        /// </summary>
        /// <param name="manifest">Manifest to add to</param>
        /// <param name="picknPlaceIPaddress">Network address of the pick and place machine</param>
        public static void AddPicknPlaceControlProxies(Manifest manifest, string picknPlaceIPaddress)
        {
            string armAgentName = null;
            string motionAgentName = null;
            string handAgentName = null;
            string armControlProxyName = "armControlProxy";
            string handControlProxyName = "handControlProxy";
            string motionPlanProxyName = "motionPlanProxy";

            foreach (Agent agent in manifest.Agents)
            {
                if (agent is IProducer<ManipulatorStatusMessage>)
                {
                    armAgentName = agent.Name;
                }
                else if (agent is SimpleCartesianMoveAgent)
                {
                    motionAgentName = agent.Name;
                }
                else if (agent is IProducer<RobotiqSModelStateMessage>)
                {
                    handAgentName = agent.Name;
                }
            }

            if (true == string.IsNullOrEmpty(armAgentName))
            {
                throw new Exception("Did not find the arm control agent");
            }

            if (true == string.IsNullOrEmpty(motionAgentName))
            {
                throw new Exception("Did not find the motion agent");
            }


            if (false == string.IsNullOrEmpty(handAgentName))
            {
                ProxyDefinition handControlProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                    name: handControlProxyName,
                    producer: handAgentName,
                    address: picknPlaceIPaddress,           // Bug Requires an IPV4 address Bug 1649
                    publishContracts: new[] { typeof(RobotiqSModelStateMessage) },
                    publishNetworkPortNumber: (int)RemotingPorts.HandPublish,
                    controlContracts: new[] { typeof(RobotiqSModelControlMessage) },
                    controlNetworkPortNumber: (int)RemotingPorts.HandControl);

                manifest.NetworkEnabledAgentProxies.Add(handControlProxy);
                manifest.ActiveAgents.Add(handControlProxy.Name);
            }

            ProxyDefinition armControlProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: armControlProxyName,
                producer: armAgentName,
                address: picknPlaceIPaddress,           // Bug Requires an IPV4 address Bug 1649
                publishContracts: new[] { typeof(ManipulatorStatusMessage) },
                publishNetworkPortNumber: (int)RemotingPorts.ArmPublish,
                controlContracts: new[] { typeof(CartesianGoalPositionControlMessage), typeof(LinearCartesianMotionControlMessage), typeof(JointGoalPositionControlMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.ArmControl);

            ProxyDefinition motionPlanProxy = ProxyDefinition.CreateNetworkEnabledProxy(
                name: motionPlanProxyName,
                producer: motionAgentName,
                address: picknPlaceIPaddress,           // Bug Requires an IPV4 address Bug 1649
                publishContracts: null,
                publishNetworkPortNumber: (int)RemotingPorts.MotionPlanPublish,
                controlContracts: new[] { typeof(CartesianGoalPositionControlMessage) },
                controlNetworkPortNumber: (int)RemotingPorts.MotionPlanControl);

            manifest.NetworkEnabledAgentProxies.Add(armControlProxy);
            manifest.NetworkEnabledAgentProxies.Add(motionPlanProxy);
            manifest.ActiveAgents.Add(armControlProxy.Name);
            manifest.ActiveAgents.Add(motionPlanProxy.Name);
        }

        public static RegionAccessibilityChecker CreateRegionAccessibilityChecker()
        {

            List<HalfSpaceRegion> inaccessibleHalfSpaces = new List<HalfSpaceRegion>();

            // the table plane which is 0.05 above the origin
            inaccessibleHalfSpaces.Add(new HalfSpaceRegion(new Vector3(0, 0, 1), new Vector3(0, 0, -0.05)));

            // the wall on the side
            inaccessibleHalfSpaces.Add(new HalfSpaceRegion(new Vector3(-1, 0, 0), new Vector3(0.7, 0, 0)));

            RegionAccessibilityChecker rc = new RegionAccessibilityChecker(null, inaccessibleHalfSpaces);

            return rc;
        }
        /// <summary>
        /// Create the control part of a pick and place using the CNC proxy
        /// </summary>
        /// <param name="manifest">Manifest to add file recording to.</param>
        /// <param name="filePath">Path to file to record to.</param>
        /// <returns>The manifest</returns>
        public static void AddManipulatorStatusFileRecorder(ref Manifest manifest, string filePath = "manipulatorStatusRecording.dat")
        {
            string armAgentName = string.Empty;

            foreach (Agent agent in manifest.Agents)
            {
                if (agent is KukaLBRControlAgent)
                {
                    armAgentName = agent.Name;
                }
            }

            if (true == string.IsNullOrEmpty(armAgentName))
            {
                throw new Exception("Did not find the arm control agent");
            }

            FileRecorderProxy manipulatorStatusRecorderAgent = new FileRecorderProxy(
                name: "ManipulatorStatusRecorderAgent",
                filePath: filePath,
                messageTypes: new Type[1] { typeof(ManipulatorStatusMessage) },
                producer: armAgentName);

            manifest.Agents.Add(manipulatorStatusRecorderAgent);
            manifest.ActiveAgents.Add(manipulatorStatusRecorderAgent.Name);
        }

        /// <summary>
        /// Create a manifest that plays back a recording of manipulation status messages and writes them to a CSV file.
        /// </summary>
        /// <param name="recordingFilePath">Path to file containing recording.</param>
        /// <param name="csvFilePath">Path to file to write csv values to.</param>
        /// <returns> A manifest. </returns>
        public static Manifest CreateGenerateCsvFromManipulatorStatusMessageRecordingManifest(string recordingFilePath = "manipulatorStatusRecording.dat", string csvFilePath = "manipulationStatusRecording.csv")
        {
            Manifest manifest = new Manifest();

            TimerAgent filePlayerTimer = new TimerAgent(
               name: "FilePlayerTimer",
               timerInterval: 1);

            FilePlayerProxy playerAgent = new FilePlayerProxy(
                "ManipulatorStatusPlayerAgent",
                recordingFilePath,
                new Type[1] { typeof(ManipulatorStatusMessage) },
                filePlayerTimer);

            ManipulatorStatusToCSVFileAgent manipulatorStatusToCSVFileAgent = new ManipulatorStatusToCSVFileAgent(
                "ManipulatorStatusToCsvFileAgent",
                playerAgent.AsProducer<ManipulatorStatusMessage>(),
                csvFilePath);

            PrintImDoneAgent imDoneAgent = new PrintImDoneAgent(
                "ImDoneAgent",
                playerAgent);

            manifest.Agents.Add(filePlayerTimer);
            manifest.Agents.Add(playerAgent);
            manifest.Agents.Add(manipulatorStatusToCSVFileAgent);
            manifest.Agents.Add(imDoneAgent);

            manifest.ActiveAgents.Add(filePlayerTimer.Name);
            manifest.ActiveAgents.Add(playerAgent.Name);
            manifest.ActiveAgents.Add(manipulatorStatusToCSVFileAgent.Name);
            manifest.ActiveAgents.Add(imDoneAgent.Name);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to visualize the force torque sensor
        /// </summary>
        /// <returns>A manifest</returns>
        public static Manifest CreateForceTorqueSensorManifest()
        {
            Manifest manifest = new Manifest();
            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 15);
            manifest.Agents.Add(timer);
            List<ViewDefinition> views = new List<ViewDefinition>();

            ATIOmega85Agent forceTorqueAgent = new ATIOmega85Agent("ATIOmega85Agent", timer, "192.168.1.1");

            ForceTorqueDisplayAgent displayAgent = new ForceTorqueDisplayAgent("FTDisplayAgent", forceTorqueAgent, 30);
            ViewDefinition def = new ViewDefinition("ForceTorqueSensorView", typeof(ForceTorqueDisplayView), displayAgent);
            views.Add(def);

            manifest.Agents.Add(forceTorqueAgent);
            manifest.Agents.Add(displayAgent);

            manifest.ActiveAgents.Add(forceTorqueAgent.Name);
            manifest.ActiveAgents.Add(displayAgent.Name);
            AddUserInterface(ref manifest, views);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to test manipulation KUKA LBR RDS sim.
        /// </summary>
        /// <returns>A manifest that enables manipulation testing on RDS sim of Kuka arm</returns>
        private static Manifest CreateKukaLbrSimManipulationTestManifestBase()
        {
            Manifest manifest = new Manifest();
            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 15);
            manifest.Agents.Add(timer);

            SimulatedKukaLBRControlAgent manipulationAgent = new SimulatedKukaLBRControlAgent(
                        name: "ManipulationAgent",
                        producer: timer,
                        maxLinearVelocityInMetersPerSec: 0.5,
                        maxAngularVelocityInRadPerSec: 0.5,
                        maxJointVelocityRadPerSec: 0.5,
                        jointMotionDurationInCycles: 11,
                        useKrlMotionCommands: false);
            manifest.Agents.Add(manipulationAgent);
            manifest.ActiveAgents.Add(manipulationAgent.Name);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to test manipulation with default kuka hardware
        /// and a simple visualization.  Includes support for splines
        /// </summary>
        /// <param name="useKrl"> Indicates whether or not to use KRL to generate motion commands.</param>
        /// <returns>A manifest that enables manipulation testing on Kuka arm</returns>
        private static Manifest CreateBasicKukaLbrManifest(bool useKrl = false)
        {
            Manifest manifest = new Manifest();
            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 15);
            manifest.Agents.Add(timer);

            KukaLBRControlAgent manipulationAgent = new KukaLBRControlAgent(
                        name: "ManipulationAgent",
                        producer: timer,
                        maxLinearVelocityInMetersPerSec: 0.2,
                        maxAngularVelocityInRadPerSec: 0.2,
                        maxJointVelocityRadPerSec: 1.0,
                        jointMotionDurationInCycles: 11,
                        useKrlMotionCommands: useKrl);
            manifest.Agents.Add(manipulationAgent);
            manifest.ActiveAgents.Add(manipulationAgent.Name);

            return manifest;
        }

        /// <summary>
        /// Create a manifest to test control of UR arm.
        /// Includes support for splines
        /// </summary>
        /// <returns>A manifest that enables control of UR arm</returns>
        private static Manifest CreateBasicURManifest()
        {
            Manifest manifest = new Manifest();
            TimerAgent timer = new TimerAgent("ManipulationHeartbeatTimer", 15);
            manifest.Agents.Add(timer);

	    // !DO NOT MODIFY THESE VALUES UNLESS YOU ARE SURE OF WHAT YOU ARE DOING!
	    // Improper settings of these values may cause uncontrolled behavior or 
            // dangerous situations including severe vibration or runaway/collisions.
            double[] pGains = new double[] { 4.0, 2.5, 2.5, 2.5, 2.5, 2.5 };
            double[] dGains = new double[] { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
            double[] deadband = new double[] { 0.001, 0.001, 0.001, 0.001, 0.001, 0.001 };
            double[] maxDelta = new double[] { 0.1, 0.1, 0.1, 0.1, 0.1, 0.1 };
            double[] maxVelocity = new double[] { 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };

            URControlAgent manipulationAgent = new URControlAgent(
                        name: "ManipulationAgent",
                        producer: timer,
                        hostname: "192.168.1.2",
                        controlServerPort: 30002,
			realTimePort: 30003,
                        pGains: pGains,
                        dGains: dGains,
                        maxJointVelocity: maxVelocity,
                        deadband: deadband,
                        maxDelta: maxDelta);

            manifest.Agents.Add(manipulationAgent);
            manifest.ActiveAgents.Add(manipulationAgent.Name);

            return manifest;
        }

        /// <summary>
        /// Run a simple test for the CHOMP algorithm with a visualization
        /// </summary>
        /// <param name="visualizerIPAddress"> IP address of the machine with the visualizer </param>
        /// <returns></returns>
        public static Manifest runsimpleCHOMPTestwithVisualization(string visualizerIPAddress)
        {
            List<ViewDefinition> views = new List<ViewDefinition>();

            // Choose the arm model to run the tests on
            ArmModel arm = ArmModel.KUKA;

            // Create the visualizer first
            Manifest manifest = ManipulationManifest.CreateStandAloneVisualizerServerLocalManifest(arm, visualizerIPAddress);

            // Get the name of the visualizer agent
            string armVisualizerAgentName = null;
            string envVisualizerAgentName = null;
            string envDisplayAgentName = null;
            foreach (Agent a in manifest.Agents)
            {
                if (a is IControllable<SimulateEETrajectoryMessage>)
                {
                    armVisualizerAgentName = a.Name;
                }
                else if (a is IControllable<VisualizeGeometryElementGroup3DMessage>)
                {
                    envVisualizerAgentName = a.Name;
                }
                else if (a is IControllable<UpdatePointCloudMessage>)
                {
                    envDisplayAgentName = a.Name;
                }
            }

            if (armVisualizerAgentName == null)
            {
                throw new ApplicationException("No arm visualizer agent found");
            }

            if (envVisualizerAgentName == null)
            {
                throw new ApplicationException("No environment visualizer agent found");
            }

            // Create the CHOMP agent next
            CHOMPExampleAgent CHOMPagent = new CHOMPExampleAgent(
                            name: "CHOMPtestagent",
                            armVisualizerAgentName: armVisualizerAgentName,
                            envVisualizerAgentName: envVisualizerAgentName,
                            envDisplayAgentName: envDisplayAgentName,
                            arm: arm);

            // Add CHOMP agent to the manifest
            manifest.Agents.Add(CHOMPagent);
            manifest.ActiveAgents.Add(CHOMPagent.Name);

            return manifest;
        }
    }
}

